/*
 *		CommandWriter.java
 *
 *		Created on 09 JAN 2005 by Duane Bolick
 *		ver 1.0
 *
 *		This class checks to see if the controller has changed the motor
 *		speeds or gripper arm states, and if it has, sends those changes to the
 *		live Khepera.  
 *
 *		Here are a list of the commands it uses to tell the Khepera what to
 *		do.  
 *
 *		Command String		What that command means to the Khepera
 *
 *		"D,x,y"				Set the left motor speed to x and the right to y
 *		"T,1,E,180"			Move the gripper arm up
 * 		"T,1,E,249"			Move the gripper arm down
 *		"T,1,D,0"			Open the gripper claw
 *		"T,1,D,1"			Close the gripper claw
 *	
 */

package edu.wsu.KheperaSimulator;

import java.io.*;
import java.util.*;

public class CommandWriter implements Runnable
{
	// String variables that'll contain the Khepera commands
	private String motorCommand;
	private String armUp;
	private String armDown;
	private String gripOpen;
	private String gripClose;
	
    private PrintWriter writer;
    private CurrentRobotState state;
	private long timeout;
 
    
    public CommandWriter( long _timeout, PrintWriter _writer, CurrentRobotState _state )
    {
    	timeout = _timeout;
        state = _state;
        writer = _writer;
        
        // The khepera commands
        motorCommand = "D";				
 		armUp = "T,1,E,180";
  		armDown = "T,1,E,249";
 		gripOpen = "T,1,D,0";
 		gripClose = "T,1,D,1";	
    }


	public void run()
	{
		while( state.sessionStatus == ClientConfiguration.RUNNING )
		{
			if( state.armChanged )
			{
				if( state.getArmState() == KSGripperStates.ARM_UP )
				{
					write( armUp );
				}
				
				else if( state.getArmState() == KSGripperStates.ARM_DOWN )
				{
					write( armDown );
				}
				
				state.armChanged = false;
			}
		
			if( state.gripChanged )
			{
				if( state.getGripperState() == KSGripperStates.GRIP_OPEN )
				{
					write( gripOpen );
				}
				
				else if( state.getGripperState() == KSGripperStates.GRIP_CLOSED )
				{
					write( gripClose );
				}
				
				state.gripChanged = false;
			}
		
			if( state.motorsChanged )
			{
				write( motorCommand + "," + (state.getMotorState()).leftSpeed + "," + (state.getMotorState()).rightSpeed );
				state.motorsChanged = false;	
			}
		
			try
			{
				Thread.sleep(timeout);
			}
			
			catch(Exception e)
			{
			}
		}
	}
   
   
  	public void write( String m )
   	{
   		if( ClientConfiguration.DEBUG ) System.out.println("commander writes: " + m);
		
		writer.println(m);
   		writer.flush();
   	}
     
   
}
